#include <chrono>
#include <iomanip>
#include <sstream>
#include <thread>
#include <filesystem>
#include "rclcpp/rclcpp.hpp"
#include "rcpputils/filesystem_helper.hpp"
#include "slam_ros/utilities.h"
#include "slam_ros/sensor_data_cache.h"
#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_cpp/writers/sequential_writer.hpp>
#include <rosbag2_storage/serialized_bag_message.hpp>
#include <rclcpp/serialization.hpp>

namespace slam_ros {

SensorDataCache::SensorDataCache(
    const std::string & sensor_data_log_path,
    const float max_time_duration)
    : sensor_data_log_path_(sensor_data_log_path),
      max_time_duration_(max_time_duration) {
  
  // Try to get log path from environment variable
  if (getEnvironmentVariable(sensor_data_log_path_, "SENSOR_DATA_LOG_DIR")) {
    RCLCPP_INFO(rclcpp::get_logger("SensorDataCache"), 
                "GetEnvironmentVariable SENSOR_DATA_LOG_DIR Successfully!");
  } else {
    RCLCPP_INFO(rclcpp::get_logger("SensorDataCache"), 
                "Using default log directory");
  }

  // Ensure path ends with '/'
  if (sensor_data_log_path_.back() != '/') {
    sensor_data_log_path_ += '/';
  }

  // Create directory if it doesn't exist
  if (!rcpputils::fs::exists(sensor_data_log_path_)) {
    if (!rcpputils::fs::create_directories(sensor_data_log_path_)) {
      RCLCPP_ERROR(rclcpp::get_logger("SensorDataCache"), 
                  "Failed to create log directory: %s", sensor_data_log_path_.c_str());
    } else {
      RCLCPP_INFO(rclcpp::get_logger("SensorDataCache"), 
                 "Created log directory: %s", sensor_data_log_path_.c_str());
    }
  }

  // Initialize buffers
  imu_data_buffer_ = std::make_unique<std::list<sensor_msgs::msg::Imu::SharedPtr>>();
  imu_data_buffer_bak_ = std::make_unique<std::list<sensor_msgs::msg::Imu::SharedPtr>>();
  odom_data_buffer_ = std::make_unique<std::list<agv_msgs::msg::OdometryLite::SharedPtr>>();
  odom_data_buffer_bak_ = std::make_unique<std::list<agv_msgs::msg::OdometryLite::SharedPtr>>();
  laser_data_buffer_ = std::make_unique<std::list<sensor_msgs::msg::LaserScan::SharedPtr>>();
  laser_data_buffer_bak_ = std::make_unique<std::list<sensor_msgs::msg::LaserScan::SharedPtr>>();
  robot_pose_buffer_ = std::make_unique<std::list<geometry_msgs::msg::PointStamped::SharedPtr>>();
  robot_pose_buffer_bak_ = std::make_unique<std::list<geometry_msgs::msg::PointStamped::SharedPtr>>();
  qrcode_buffer_ = std::make_unique<std::list<slam_ros_msgs::msg::QRCode::SharedPtr>>();
  qrcode_buffer_bak_ = std::make_unique<std::list<slam_ros_msgs::msg::QRCode::SharedPtr>>();
}

SensorDataCache::~SensorDataCache() {
  // Smart pointers will automatically clean up
}

void SensorDataCache::AddLaserData(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
  laser_data_buffer_->push_back(msg);
  while (rclcpp::Time(laser_data_buffer_->back()->header.stamp) - 
         rclcpp::Time(laser_data_buffer_->front()->header.stamp) > 
         rclcpp::Duration::from_seconds(max_time_duration_)) {
    laser_data_buffer_->pop_front();
  }
}

void SensorDataCache::AddImuData(const sensor_msgs::msg::Imu::SharedPtr msg) {
  imu_data_buffer_->push_back(msg);
  while (rclcpp::Time(imu_data_buffer_->back()->header.stamp) - 
         rclcpp::Time(imu_data_buffer_->front()->header.stamp) > 
         rclcpp::Duration::from_seconds(max_time_duration_)) {
    imu_data_buffer_->pop_front();
  }
}

void SensorDataCache::AddOdomData(const agv_msgs::msg::OdometryLite::SharedPtr msg) {
  odom_data_buffer_->push_back(msg);
  while (rclcpp::Time(odom_data_buffer_->back()->header.stamp) - 
         rclcpp::Time(odom_data_buffer_->front()->header.stamp) > 
         rclcpp::Duration::from_seconds(max_time_duration_)) {
    odom_data_buffer_->pop_front();
  }
}

void SensorDataCache::AddRobotPose(const geometry_msgs::msg::PointStamped::SharedPtr msg) {
  robot_pose_buffer_->push_back(msg);
  while (rclcpp::Time(robot_pose_buffer_->back()->header.stamp) - 
         rclcpp::Time(robot_pose_buffer_->front()->header.stamp) > 
         rclcpp::Duration::from_seconds(max_time_duration_)) {
    robot_pose_buffer_->pop_front();
  }
}

void SensorDataCache::AddQRCode(const slam_ros_msgs::msg::QRCode::SharedPtr msg) {
  qrcode_buffer_->push_back(msg);
  while (rclcpp::Time(qrcode_buffer_->back()->header.stamp) - 
         rclcpp::Time(qrcode_buffer_->front()->header.stamp) > 
         rclcpp::Duration::from_seconds(max_time_duration_)) {
    qrcode_buffer_->pop_front();
  }
}

float SensorDataCache::GetBufferMaxLengthInSec() {
  float imu_data_length = 0.0;
  float odom_data_length = 0.0;
  float laser_data_length = 0.0;

  if (!imu_data_buffer_->empty()) {
    imu_data_length = (rclcpp::Time(imu_data_buffer_->back()->header.stamp) - 
                      rclcpp::Time(imu_data_buffer_->front()->header.stamp)).seconds();
  }
  
  if (!odom_data_buffer_->empty()) {
    odom_data_length = (rclcpp::Time(odom_data_buffer_->back()->header.stamp) - 
                       rclcpp::Time(odom_data_buffer_->front()->header.stamp)).seconds();
  }
  
  if (!laser_data_buffer_->empty()) {
    laser_data_length = (rclcpp::Time(laser_data_buffer_->back()->header.stamp) - 
                        rclcpp::Time(laser_data_buffer_->front()->header.stamp)).seconds();
  }

  return std::max({imu_data_length, odom_data_length, laser_data_length});
}

void SensorDataCache::SwapSensorData() {
  if (!laser_data_buffer_->empty()) {
    std::swap(laser_data_buffer_, laser_data_buffer_bak_);
  } else {
    RCLCPP_WARN(rclcpp::get_logger("SensorDataCache"), "Empty laser_data_buffer!");
  }

  if (!odom_data_buffer_->empty()) {
    std::swap(odom_data_buffer_, odom_data_buffer_bak_);
  } else {
    RCLCPP_WARN(rclcpp::get_logger("SensorDataCache"), "Empty odom_data_buffer!");
  }

  if (!imu_data_buffer_->empty()) {
    std::swap(imu_data_buffer_, imu_data_buffer_bak_);
  } else {
    RCLCPP_WARN(rclcpp::get_logger("SensorDataCache"), "Empty imu_data_buffer!");
  }

  if (!robot_pose_buffer_->empty()) {
    std::swap(robot_pose_buffer_, robot_pose_buffer_bak_);
  } else {
    RCLCPP_WARN(rclcpp::get_logger("SensorDataCache"), "Empty robot_pose_buffer!");
  }

  if (!qrcode_buffer_->empty()) {
    std::swap(qrcode_buffer_, qrcode_buffer_bak_);
  } else {
    RCLCPP_WARN(rclcpp::get_logger("SensorDataCache"), "Empty qrcode_buffer!");
  }
}

void SensorDataCache::LogSensorData() {
  if (!rcpputils::fs::exists(sensor_data_log_path_)) {
    RCLCPP_ERROR(rclcpp::get_logger("SensorDataCache"), 
                "Sensor data log directory not found: %s", sensor_data_log_path_.c_str());
    return;
  }

  const auto now = std::chrono::system_clock::now();
  const std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
  std::tm current_time = {};
  localtime_r(&now_time_t, &current_time);
  
  std::stringstream ss;
  ss << std::put_time(&current_time, "%Y-%m-%d-%H-%M-%S");
  const std::string bag_name = sensor_data_log_path_ + ss.str();

  rosbag2_cpp::Writer writer;
  
  rosbag2_storage::StorageOptions storage_options;
  storage_options.uri = bag_name;
  storage_options.storage_id = "sqlite3";
  
  rosbag2_cpp::ConverterOptions converter_options;
  converter_options.input_serialization_format = "cdr";
  converter_options.output_serialization_format = "cdr";
  
  try {
    writer.open(storage_options, converter_options);
    
    auto write_topic = [&](auto& buffer, const std::string& topic_name, const std::string& type) {
      if (!buffer->empty()) {
        writer.create_topic({topic_name, type, "cdr", ""});
        
        // Create type-specific serialization
        using MessageT = typename std::remove_reference<decltype(*buffer->front())>::type;
        rclcpp::Serialization<MessageT> serialization;
        
        for (const auto& msg : *buffer) {
          auto serialized_msg = std::make_shared<rosbag2_storage::SerializedBagMessage>();
          
          rclcpp::SerializedMessage serialized_data;
          serialization.serialize_message(msg.get(), &serialized_data);
          
          serialized_msg->serialized_data = std::make_shared<rcutils_uint8_array_t>(
            serialized_data.release_rcl_serialized_message());
          
          // Handle different message types for timestamp extraction
          if constexpr (std::is_member_pointer<decltype(&MessageT::header)>::value) {
            serialized_msg->time_stamp = rclcpp::Time(msg->header.stamp).nanoseconds();
          } else {
            // Fallback to current time if no header available
            serialized_msg->time_stamp = now.time_since_epoch().count();
          }
          
          serialized_msg->topic_name = topic_name;
          writer.write(serialized_msg);
        }
      }
    };

    write_topic(laser_data_buffer_bak_, "/scan", "sensor_msgs/msg/LaserScan");
    write_topic(odom_data_buffer_bak_, "/odom", "agv_msgs/msg/OdometryLite");
    write_topic(imu_data_buffer_bak_, "/imu", "sensor_msgs/msg/Imu");
    write_topic(robot_pose_buffer_bak_, "/robot_pose", "geometry_msgs/msg/PoseStamped");
    write_topic(qrcode_buffer_bak_, "/qr_code", "agv_msgs/msg/QRCode");
    
    writer.close();
    RCLCPP_INFO(rclcpp::get_logger("SensorDataCache"), 
               "Sensor data written to: %s", bag_name.c_str());
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("SensorDataCache"), 
                "Failed to write bag: %s", e.what());
  }
  
  laser_data_buffer_bak_->clear();
  odom_data_buffer_bak_->clear();
  imu_data_buffer_bak_->clear();
  robot_pose_buffer_bak_->clear();
  qrcode_buffer_bak_->clear();
}

} // namespace slam_ros